#!/usr/bin/env python

# Copyright (c) 2018 Intel Labs.
# authors: German Ros (german.ros@intel.com)
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
"""
1.从Map/obstacle.txt文件中读入障碍车生成位置
2.可以通过 -m xxx读入xxx名字的opendrive格式
3.本车的生成点在Map/spawn.txt文件中读入（第一行）
4.本车的目的地点在Map/spawn.txt文件中读入（第2行开始）

启动方法：
1.在terminal中输入 start_carla 是(alias)指令
2.在另一个terminal中输入 cd_carla 同样也是（alias）指令
"""

"""Example of automatic vehicle control from client side."""

# from __future__ import print_function

import matplotlib.pyplot as plt

import argparse
import collections
import datetime
import glob
import logging
import math
import os
import random
import re
import sys
import weakref

try:
    import numpy as np
except ImportError:
    raise RuntimeError(
        'cannot import numpy, make sure numpy package is installed')

# ==============================================================================
# -- Find CARLA module ---------------------------------------------------------
# ==============================================================================
try:
    carla_dir = glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]
    print('carla_dir=', carla_dir)
    sys.path.append(carla_dir)
except IndexError:
    print('carla not found')
    pass

# ==============================================================================
# -- Add PythonAPI for release mode --------------------------------------------
# ==============================================================================
try:
    sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla')
except IndexError:
    pass

import carla
from carla import ColorConverter as cc

from agents.navigation.AD_RTK_agent import ADRTKAgent
from agents.tools.misc import spawn_vehilce_at_point, set_bird_view
from agents.simulation.set_static_obstacle import SetStaticObstacles

# ==============================================================================
# -- Global functions ----------------------------------------------------------
# ==============================================================================
def get_actor_display_name(actor, truncate=250):
    """Method to get actor display name"""
    name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:])
    return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name

# ==============================================================================
# -- spawn hero vehicle ---------------------------------------------------------
# ==============================================================================
def spawn_hero(world):
    """
    spawn the main hero vehicle
    """
    blueprint = world.get_blueprint_library().filter('*vehicle')[2] # CarlaOk
    blueprint.set_attribute('role_name', 'hero')
    if blueprint.has_attribute('color'):
        color = random.choice(blueprint.get_attribute('color').recommended_values)
        blueprint.set_attribute('color', color)

    spawn_point = None
    hero = None
    while hero is None:
        #  get spawnpoint/des from file
        # 产生主控车辆
        spawn_file = "../map/spawn.txt"
        posi = []
        with open(spawn_file) as sf:
            localizations = sf.readlines()
            for loc in localizations:
                loc = loc.strip('\n')
                posi.append(loc.split(','))

        point = carla.Location(int(posi[0][0]), int(posi[0][1]), 0)
        wp = world.get_map().get_waypoint(point)
        print(wp.transform.location)
        hero = spawn_vehilce_at_point(world, point, blueprint)
        hero.apply_control(carla.VehicleControl(brake=1.0))
    return hero

# ==============================================================================
# -- spawn static obstacles ---------------------------------------------------------
# ==============================================================================
def spawn_static_obstacles(world):
    """
    生成静态障碍物
    """
    static_obs_location = []
    static_obstacles = []
    try:
        ob_file = open('../map/static_obstacle.txt')
        obs_location_txt = ob_file.readlines()
        for location in obs_location_txt:
            location = location.strip('\n')
            location = location.split(',')
            static_obs_location.append(carla.Location(int(location[0]), int(location[1]), 0))
    except FileNotFoundError:
        print('no static obstacles')
    print('static_obs_location=', static_obs_location)

    if static_obs_location:            
        for point in static_obs_location:
            blueprint = world.get_blueprint_library().filter("*vehicle")[1]
            static_obstacles.append(spawn_vehilce_at_point(world, point, blueprint))
            static_obstacles[-1].apply_control(carla.VehicleControl(brake=1.0))
    return static_obstacles

# ==============================================================================
# -- Game Loop ---------------------------------------------------------
# ==============================================================================
def spawn_moving_obstacles(world):
    # spawn obstacle vehicle if the file exist
    obs_location = []
    vehicles = []
    try:
        ob_file = open('../map/obstacle.txt')
        obs_location_txt = ob_file.readlines()
        for location in obs_location_txt:
            location = location.strip('\n')
            location = location.split(',')
            obs_location.append(carla.Location(int(location[0]), int(location[1]), 0))
    except FileNotFoundError:
        print('no obstacles')
        
    if obs_location:            
        for point in obs_location:
            # blueprint = random.choice(self.world.get_blueprint_library().filter("*vehicle"))
            blueprint = world.get_blueprint_library().filter("*vehicle")[0]
            vehicles.append(spawn_vehilce_at_point(world, point, blueprint))
            vehicles[-1].apply_control(carla.VehicleControl(brake=1.0))
    return vehicles

# ==============================================================================
# -- Game Loop ---------------------------------------------------------
# ==============================================================================
def game_loop(args):
    """ Main loop for agent"""
    world = None
    tot_target_reached = 0
    num_min_waypoints = 21

    try:
        client = carla.Client(args.host, args.port)

        client.set_timeout(4.0) # 连接超时判断时间

        # if loading xodr is nessarry, then load it and regenerate the world
        if args.map != None:
            xodr_path = '../map/' + args.map + '.xodr'
            print(xodr_path)
            with open(xodr_path) as od_file:
                try:
                    data = od_file.read()
                except OSError:
                    print('file could not be readed.')
                    sys.exit()
            vertex_distance = 2.0  # in meters
            max_road_length = 50.0 # in meters
            wall_height = 5.0      # in meters
            extra_width = 0.6      # in meters
            client.generate_opendrive_world(
                data, carla.OpendriveGenerationParameters(
                    vertex_distance=vertex_distance,
                    max_road_length=max_road_length,
                    wall_height=wall_height,
                    additional_width=extra_width,
                    smooth_junctions=True,
                    enable_mesh_visibility=True)) # 生成新的world
        
        client.reload_world()
        world = client.get_world() #
        settings = world.get_settings()
        settings.fixed_delta_seconds = 0.02 # 50 fps
        world.apply_settings(settings)
        
        town_map = world.get_map()
        """
        1.重启world
        2.生成障碍物
        3.生成hero
        4.开启控制
        """
        # world.restart()
        # static
        static_obstacles = spawn_static_obstacles(world)
        # moving
        moving_obstacles = spawn_moving_obstacles(world)
        # hero
        hero = spawn_hero(world)        

        # 在这里进行其他仿真车辆的控制
        if moving_obstacles:
            for vehicle in moving_obstacles:
                vehicle.set_autopilot()

        # 获取车辆行驶的目的地
        spawn_file = "../map/spawn.txt"
        posi = []
        point = None
        with open(spawn_file) as sf:
            localizations = sf.readlines()
            for loc in localizations:
                loc = loc.strip('\n')
                posi.append(loc.split(','))
            point = carla.Location(int(posi[1][0]), int(posi[1][1]), 0)

        if point:
            destination = town_map.get_waypoint(point).transform.location
        else:
            spawn_point = town_map.get_spawn_points()[0]
            destination = carla.location(spawn_point.location.x,
                                   spawn_point.location.y,
                                   spawn_point.location.z)

        # 实例化控制方法：agent = 
        agent = ADRTKAgent(hero, behavior=args.behavior)
        agent.set_perception(static_obstacles, moving_obstacles)

        # 把全局规划放在agent中的好处是每辆车自己来做全局规划，可以认为agent就是司机
        agent.set_destination(agent.vehicle.get_location(), destination, clean=True)
        agent.display_global_trace(True)

        # 准备启动车辆,预装载路径
        agent._local_planner.init_local_trace()
        
        spectator = world.get_spectator()
        set_bird_view(spectator, hero)

        time_stamp = 0.0
        while True:            
            # 如果时间超过 agent._display_time，则再次显示一次全局路径
            time_stamp += world.wait_for_tick().timestamp.delta_seconds
            if time_stamp >= 5.0:
                time_stamp = 0.0
                agent.display_global_trace(True)
                set_bird_view(spectator, hero)
            # # As soon as the server is ready continue!
            # if not world.wait_for_tick(10.0):
            #     continue

            # Set new destination when target has been reached
            if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop:
                pass                    

            elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop:
                print("Target reached, mission accomplished...")
                break

            speed_limit = 120.0 # 车辆自身限速
            agent.get_local_planner().set_speed(speed_limit)

            control = agent.run_step(debug=True) # 调用agent方法计算
            hero.apply_control(control)

    finally:
        # while hero:
        #     hero.destroy()
        # while static_obstacles:
        #     static_obstacles[-1].destroy()
        # while moving_obstacles:
        #     moving_obstacles[-1].destroy()
        print('exit')

# ==============================================================================
# -- main() --------------------------------------------------------------
# ==============================================================================
def main():
    """Main method"""

    argparser = argparse.ArgumentParser(
        description='CARLA Automatic Control Client')
    argparser.add_argument(
        '-v', '--verbose',
        action='store_true',
        dest='debug',
        help='Print debug information')
    argparser.add_argument(
        '--host',
        metavar='H',
        default='127.0.0.1',
        help='IP of the host server (default: 127.0.0.1)')
    argparser.add_argument(
        '-p', '--port',
        metavar='P',
        default=2000,
        type=int,
        help='TCP port to listen to (default: 2000)')
    argparser.add_argument(
        '--res',
        metavar='WIDTHxHEIGHT',
        default='720x480',
        help='Window resolution (default: 720x480)')
    argparser.add_argument(
        '--filter',
        metavar='PATTERN',
        default='vehicle.*',
        help='Actor filter (default: "vehicle.*")')
    argparser.add_argument(
        '--gamma',
        default=2.2,
        type=float,
        help='Gamma correction of the camera (default: 2.2)')
    argparser.add_argument(
        '-l', '--loop',
        action='store_true',
        dest='loop',
        help='Sets a new random destination upon reaching the previous one (default: False)')
    argparser.add_argument(
        '-b', '--behavior', type=str,
        choices=["cautious", "normal", "aggressive"],
        help='Choose one of the possible agent behaviors (default: normal) ',
        default='normal')
    argparser.add_argument("-a", "--agent", type=str,
                           choices=["Behavior", "Roaming", "Basic", "MPC"],
                           help="select which agent to run",
                           default="MPC")
    argparser.add_argument(
        '-s', '--seed',
        help='Set seed for repeating executions (default: None)',
        default=None,
        type=int)
    argparser.add_argument("-m", "--map", type=str,
                           help="Set the xodr map name to use XODR-ONLY mode",
                           default=None)

    args = argparser.parse_args()

    args.width, args.height = [int(x) for x in args.res.split('x')]

    log_level = logging.DEBUG if args.debug else logging.INFO
    logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level)

    logging.info('listening to server %s:%s', args.host, args.port)

    print(__doc__)

    try:
        game_loop(args)

    except KeyboardInterrupt:
        print('\nCancelled by user. Bye!')


if __name__ == '__main__':
    main()
